package com.grt192.mechanism;

import java.util.Vector;

import com.grt192.core.Mechanism;
import com.grt192.event.SensorChangeListener;
import com.grt192.event.SensorEvent;
import com.grt192.mechanism.component.DriverStationListener;
import com.grt192.sensor.DemoJoystick;
import com.grt192.sensor.spot.NetServantSensor;

/**
 * A SPOTDriverStation
 * 
 * @author Andrew Chen <andrewtheannihilator@gmail.com>
 */
public class SPOTDriverStation extends Mechanism implements
		SensorChangeListener {
	private static final double DEADZONE = .2;

	private final NetServantSensor joystick;
	private double x, y;
	private Vector listeners;

	public SPOTDriverStation(NetServantSensor joystick) {
		this.joystick = joystick;
		joystick.addSensorChangeListener(this);
		// joystick.start();
		listeners = new Vector();
	}

	/**
	 * Gets the network-read joystick
	 * 
	 * @return
	 */
	public NetServantSensor getJoystick() {
		return joystick;
	}

	/**
	 * Converts a joystick read radian angle to a useful percentage of possible
	 * control
	 * 
	 * @param angle
	 *            an angle [-pi/2... pi/2]
	 * @return a percentage [-1... 1]
	 */
	private double toPercent(double angle) {
		return angle / (Math.PI / 2);
	}

	/**
	 * Receives raw data from the sensor
	 */
	public void sensorStateChanged(SensorEvent e, String key) {
		if (key.equals(DemoJoystick.X)) {
			double x = e.getData(key);
			this.x = Math.abs(x) > DEADZONE ? x : 0;
		} else if (key.equals(DemoJoystick.Y)) {
			double y = e.getData(key);
			this.y = Math.abs(y) > DEADZONE ? y : 0;
		}
		notifyListeners();
	}

	/**
	 * 
	 */
	public void notifyListeners() {
		for (int i = 0; i < listeners.size(); i++) {
			((DriverStationListener) listeners.elementAt(i)).joystickMoved(
					joystick, x, y);
		}
	}

	/**
	 * Adds this obect to receive percentage events from the joystick
	 * 
	 * @param s
	 */
	public void addDSListener(DriverStationListener s) {
		listeners.add(s);
	}

	/**
	 * Removes this obect to receive percentage events from the joystick
	 * 
	 * @param s
	 */
	public void removeDSListener(DriverStationListener s) {
		listeners.remove(s);
	}
}
